/**
 * Copyright (C) BFH www.bfh.ch 2011
 * Code written by: Patrick Dobler, Marc Folly
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package ch.bfh.ti.kybernetik.engine.controller.roboter;

import javax.vecmath.Point2d;
import javax.vecmath.Vector2d;

import ch.bfh.ti.kybernetik.engine.VecMathUtils;
import ch.bfh.ti.kybernetik.engine.controller.Simulator;
import ch.bfh.ti.kybernetik.engine.model.Roboter;
import ch.bfh.ti.kybernetik.engine.model.RoboterElement;

/**
 * The {@link Simulator} default {@link RoboterConstruction} implementation
 * 
 */
public class DefaultRoboterConstructionImpl implements RoboterConstruction {

	private final Roboter roboter;

	public DefaultRoboterConstructionImpl(Roboter roboter) {
		super();
		this.roboter = roboter;
	}

	@Override
	public Point2d getRoboterElementPoint(RoboterElement roboterElements) {
		return getRoboterRoboterElementPoint(new Point2d(roboter.getX(), roboter.getY()), roboter.getDirection(),
				roboterElements.getDistanceX());
	}

	private Point2d getRoboterRoboterElementPoint(Point2d roboterPoint, Vector2d direction, double distance) {
		Vector2d horiziontalVector = buildRightAngleVector(direction);
		double x = roboterPoint.getX() + (distance) * horiziontalVector.getX();
		double y = roboterPoint.getY() + (distance) * horiziontalVector.getY();
		return new Point2d(x, y);
	}

	@Override
	public Vector2d getRoboterElementAngleVector(RoboterElement roboterElement) {
		double sensorAngle = roboterElement.getElementAngle();
		Vector2d sensorVector = VecMathUtils.rotateVector(roboter.getDirection(), sensorAngle);
		return sensorVector;

	}

	private Vector2d buildRightAngleVector(Vector2d vector) {
		Vector2d horizontalVector = new Vector2d(-vector.getY(), vector.getX());
		horizontalVector.normalize();
		return horizontalVector;
	}

}
